#include "mm.h"

//in task 1 ,want  vaddr:0x000000 ~ 0xfffff to paddr:0x20000000 ~ 0x200fffff
//exec 0 0x10000 0x20000 0x10000 0x20000
uint32_t PFN = 0x20000;
void init_page_table()
{
    int i;
    uint32_t VPN2 = 0x00;
    uint32_t ASID = 0x00;
    //uint32_t PFN = 0x00;
    uint32_t C = 2;
    uint32_t D = 1;
    uint32_t V = 1;
    uint32_t G = 1;
    for(i = 0; i < 128; i++){
        task1_table[i].mapping = 1;
        task1_table[i].Hi = (VPN2 << 13 | ASID) & 0x000000ffffffe0ff;
        task1_table[i].Lo0 = ((PFN) << 6 | C << 3 | D << 2 | V << 1 | G) & 0x000000003fffffff;
        task1_table[i].Lo1 = ((PFN + 1) << 6 | C << 3 | D << 2 | V << 1 | G) & 0x000000003fffffff;
        VPN2 ++;
        PFN = PFN + 2;
    }
}

void init_page_table_task3()
{
    int i;
    for(i = 0; i < 128; i++){
        task1_table[i].mapping = 0;
        task1_table[i].in_tlb = 0;
    }
    uint32_t VPN2 = 0x00;
    uint32_t ASID = 0x00;
    //uint32_t PFN = 0x00;
    uint32_t C = 2;
    uint32_t D = 1;
    uint32_t V = 1;
    uint32_t G = 1;
    for(i = 0; i < 128; i++){
        task1_table[i].Hi = (VPN2 << 13 | ASID) & 0x000000ffffffe0ff;
        // task1_table[i].Lo0 = ((PFN) << 6 | C << 3 | D << 2 | V << 1 | G) & 0x000000003fffffff;
        // task1_table[i].Lo0 = ((PFN + 1) << 6 | C << 3 | D << 2 | V << 1 | G) & 0x000000003fffffff;
        VPN2 ++;
        // PFN = PFN + 2;
    }
}

int refill_index = 0;
void do_TLB_Refill()
{
    uint32_t VPN2;
    uint32_t ASID;
    uint32_t C = 2;
    uint32_t D = 1;
    uint32_t V = 1;
    uint32_t G = 0;
    uint64_t test_context;
    test_context = get_cp0_context();
    VPN2 = (get_cp0_context() & 0x007ffff0) >> 4;
    if(task1_table[VPN2].mapping){//have mapping but not in tlb
        if(task1_table[VPN2].in_tlb){
            printk("-----Mem Protect!----\n");
            while(1);
        }
        set_cp0_pagemask(0x0);
        set_cp0_entryhi(task1_table[VPN2].Hi);
        set_cp0_entrylo0(task1_table[VPN2].Lo0);
        set_cp0_entrylo1(task1_table[VPN2].Lo1);
        set_cp0_index(refill_index);
        task1_table[VPN2].in_tlb = 1;
    }
    else {//have no mapping
        printk("----Adding to pagetable!----\n");
        printk("-----0x%x-------\n", VPN2);
        set_cp0_pagemask(0x0);
        ASID = do_getpid();
        task1_table[VPN2].Hi = task1_table[VPN2].Hi | ASID;
        task1_table[VPN2].Lo0 = ((PFN) << 6 | C << 3 | D << 2 | V << 1 | G) & 0x000000003fffffff;
        task1_table[VPN2].Lo1 = ((PFN + 1) << 6 | C << 3 | D << 2 | V << 1 | G) & 0x000000003fffffff;
        task1_table[VPN2].mapping = 1;
        task1_table[VPN2].in_tlb = 1;
        PFN = PFN + 2;
        set_cp0_entryhi(task1_table[VPN2].Hi);
        set_cp0_entrylo0(task1_table[VPN2].Lo0);
        set_cp0_entrylo1(task1_table[VPN2].Lo1);
        set_cp0_index(refill_index);
    }
    refill_index++;
    if(refill_index > 63){
        refill_index = 0;
    }
    asm volatile ("tlbwi");
}

void do_TLB_Invalid()
{
    //printk("-------TLB_Invalid-----\n");
    uint32_t VPN2;
    uint32_t ASID;
    uint32_t C = 2;
    uint32_t D = 1;
    uint32_t V = 1;
    uint32_t G = 1;
    uint32_t judge;
    VPN2 = (get_cp0_context() & 0x007ffff0) >> 4;
    set_cp0_pagemask(0x0);
    set_cp0_entryhi(task1_table[VPN2].Hi);
    if(task1_table[VPN2].mapping){
        judge = ((task1_table[VPN2].Lo0) & 0x00000002) & ((task1_table[VPN2].Lo1) & 0x00000002);
        if(judge){
            set_cp0_entrylo0(task1_table[VPN2].Lo0);
            set_cp0_entrylo1(task1_table[VPN2].Lo1);
        }
        else {
            ASID = do_getpid();
            task1_table[VPN2].Hi = task1_table[VPN2].Hi | ASID;
            task1_table[VPN2].Lo0 = ((PFN) << 6 | C << 3 | D << 2 | V << 1 | G) & 0x000000003fffffff;
            task1_table[VPN2].Lo1 = ((PFN + 1) << 6 | C << 3 | D << 2 | V << 1 | G) & 0x000000003fffffff;
            task1_table[VPN2].mapping = 1;
            task1_table[VPN2].in_tlb = 1;
            PFN = PFN + 2;
            set_cp0_entryhi(task1_table[VPN2].Hi);
            set_cp0_entrylo0(task1_table[VPN2].Lo0);
            set_cp0_entrylo1(task1_table[VPN2].Lo1);
        }
    }
   else {
        ASID = do_getpid();
        task1_table[VPN2].Hi = task1_table[VPN2].Hi | ASID;
        task1_table[VPN2].Lo0 = ((PFN) << 6 | C << 3 | D << 2 | V << 1 | G) & 0x000000003fffffff;
        task1_table[VPN2].Lo1 = ((PFN + 1) << 6 | C << 3 | D << 2 | V << 1 | G) & 0x000000003fffffff;
        task1_table[VPN2].mapping = 1;
        task1_table[VPN2].in_tlb = 1;
        PFN = PFN + 2;
        set_cp0_entryhi(task1_table[VPN2].Hi);
        set_cp0_entrylo0(task1_table[VPN2].Lo0);
        set_cp0_entrylo1(task1_table[VPN2].Lo1);
   }
    
    asm volatile("tlbp");

    uint32_t index;
    uint32_t index_p;
    index = get_cp0_index();
    index_p = index & 0x80000000;
    if(index_p){
        printk("Can't find index!\n");
        while(1);
    }
    index &= 0x0000003f;
    set_cp0_index(index);
     
    asm volatile ("tlbwi");
}

void do_page_fault()
{
}
void init_TLB(void)
{
    //init_page_table();
    uint64_t entryhi;
    uint64_t entrylo0;
    uint64_t entrylo1;
    uint32_t index;

    int i;
    set_cp0_pagemask(0x0);
    for(i = 0; i < 64; i++){
        entryhi = task1_table[i].Hi;
        entrylo0 = task1_table[i].Lo0;
        entrylo1 = task1_table[i].Lo1;
        index = i;

        set_cp0_entryhi(entryhi);
        set_cp0_entrylo0(entrylo0);
        set_cp0_entrylo1(entrylo1);
        set_cp0_index(index);

        asm volatile ("tlbwi");
    }
}
void physical_frame_initial(void)
{
}